package intelligent_driver;

import java.util.ArrayList;

public class Lane {
	
	private ArrayList<Vehicle> vq;
	
	private int length;
	
	public Lane(int l){
		length = l;
		vq = new ArrayList<Vehicle>();
	}

	public void addVehicle(Vehicle v){
		vq.add(v);
	}
	
	public void moveVehicles(double dt){
		Vehicle v;
		Vehicle vf;
		int delta = 4;
		double v0;//desired velocity in free traffic (=topSpeed)
		double sa;//net distance x_i-1 - x_i - l_i-1
		double s0 = 5;//minimum spacing
		double T = 1;//desired time headway in s
		double b = 20;//comfortable braking deceleration in m/s^2
		double dva;//approaching rate v_i - v_i-1
		double sstar;
		for(int i=0; i < vq.size(); i++){
			if(i==0){ //free ride, no cars ahead, later implement connected lanes and intersections
				v = vq.get(i);
				v.setPosition(v.position() + v.velocity() * dt);
				v0 = v.topSpeed();
				v.setVelocity(v.acceleration() * (1 - Math.pow((v.velocity()/v0), delta)) * dt);
				//System.out.println("v"+i+": "+v.position());
			}
			else{
				v = vq.get(i);
				vf = vq.get(i-1);
				v.setPosition(v.position() + v.velocity() * dt);
				v0 = v.topSpeed();
				sa = vf.position() - v.position() - vf.length();
				dva = v.velocity() - vf.velocity();
				sstar = s0 + v.velocity() * T + (v.velocity()*dva)/(2*Math.sqrt(v.acceleration()*b));
				v.setVelocity(v.acceleration() * (1 - Math.pow((v.velocity()/v0), delta) - Math.pow((sstar/sa), 2)) * dt);
				//System.out.println("v"+i+": "+v.position());
			}
		}	
	}
	
	public ArrayList<Vehicle> getVq(){
			return vq;
	}
}
